#!/usr/bin/env python3
"""
@Author: wujin_xueyk\n
@Date:   2022-01-01\n
@Function: conversion between different geographic coordinates!\n
@Refs:\n
1. See other similar libs like GeographicLib.\n
"""
import sys
import math

earth_radius_a = 6378137.0            # equatorial radius(semi-major axis)
earth_radius_b = 6356752.3142         # polar radius(semi-minor axis)


def cc_geodetic2ecef(gps):
    """
    Convert from geodetic coordinate to ECEF coordinate.

    Input:
        - gps: (lat, lon, alt)
        - lat: latitude, in degree
        - lon: longitude, in degree
        - alt: altitude, in meter
    Output:
        (X, Y, Z), in meter
    """
    lat = math.radians(gps[0])
    lon = math.radians(gps[1])
    alt = gps[2]
    N = earth_radius_a**2 / \
        math.sqrt(earth_radius_a**2 * math.cos(lat)**2 +
                  earth_radius_b**2 * math.sin(lat)**2)
    X = (N + alt) * math.cos(lat) * math.cos(lon)
    Y = (N + alt) * math.cos(lat) * math.sin(lon)
    Z = (earth_radius_b**2/earth_radius_a**2 * N + alt) * math.sin(lat)

    return X, Y, Z


def cc_ecef2geodetic(pos):
    """
    Convert from ECEF coordinate to geodetic coordinate.

    Input:
        - pos: (X, Y, Z), in meter
    Output:
        gps=(lat, lon, alt)
        lat: latitude, in degree
        lon: longitude, in degree
        alt: altitude, in meter
    """
    X, Y, Z = pos
    a = earth_radius_a
    b = earth_radius_b
    c = math.sqrt((a**2-b**2)/a**2)
    d = math.sqrt((a**2-b**2)/b**2)
    p = math.sqrt(X**2 + Y**2)
    q = math.atan2(Z*a, p*b)
    lon = math.atan2(Y, X)
    lat = math.atan2((Z+d**2*b*math.pow(math.sin(q), 3)),
                     (p-c**2*a*math.pow(math.cos(q), 3)))
    N = a / math.sqrt(1 - (c**2*math.pow(math.sin(lat), 2)))
    alt = p / math.cos(lat) - N
    lon = math.degrees(lon)
    lat = math.degrees(lat)

    return lat, lon, alt

    # X, Y, Z = pos
    # a  = earth_radius_a
    # b  = earth_radius_b
    # e  = 1 - b**2/a**2                                                                                #
    # e_der = (a**2 - b**2)/b**2                                                                        #
    # p     = math.sqrt(X**2 + Y**2)                                                                    #

    # F     = 54*b**2*Z**2
    # G     = p**2 + (1-e**2)*Z**2 - e**2*(a**2-b**2)
    # c     = e**4 * F * p**2 / G**3
    # s     = math.pow(1+c+math.sqrt(c**2+2*c), 1.0/3)
    # k     = s + 1 + 1.0/s
    # P     = F / (3*k**2*G**2)
    # Q     = math.sqrt(1 + 2*e**4*P)
    # r_0   = -P*e**2*p/(1+Q) + math.sqrt(0.5*a**2*(1+1.0/Q)-P*(1-e**2)*Z**2/(Q*(1+Q)) - 0.5*P*p**2)     #
    # U     = math.sqrt((p-e**2*r_0)**2 + Z**2)
    # V     = math.sqrt((p-e**2*r_0)**2 + (1-e**2)*Z**2)                                                 #
    # z_0   = b**2 * Z / (a * V)                                                                         #

    # alt   = U * (1 - b**2/(a*V))
    # lat   = math.atan((Z + e_der**2 * z_0) / p)
    # lon   = math.atan2(Y, X)

    # lat   = math.degrees(lat)
    # lon   = math.degrees(lon)
    # return lat, lon, alt


def cc_ecef2enu(orig, pos):
    """
    Convert from ECEF coordinate to ENU coordinate.

    Input:
        - orig: the coordinate of enu frame's origin in ECEF
        - pos : (X,Y,Z), a point represent in ECEF
    Output:
        pos: (x,y,z), the location of the point represent in ENU
    """
    X_o = orig[0]
    Y_o = orig[1]
    Z_o = orig[2]
    X = pos[0]
    Y = pos[1]
    Z = pos[2]

    orig_gps = cc_ecef2geodetic(orig)

    lat = math.radians(orig_gps[0])
    lon = math.radians(orig_gps[1])
    alt = orig_gps[2]

    x = -math.sin(lon) * (X-X_o) + math.cos(lon) * (Y-Y_o)
    y = -math.sin(lat) * math.cos(lon) * (X-X_o) - math.sin(lat) * \
        math.sin(lon) * (Y-Y_o) + math.cos(lat) * (Z-Z_o)
    z = math.cos(lat) * math.cos(lon) * (X-X_o) + math.cos(lat) * \
        math.sin(lon) * (Y-Y_o) + math.sin(lat) * (Z-Z_o)

    return x, y, z


def cc_enu2ecef(orig, pos):
    """
    Convert from ENU coordinate to ECEF coordinate.

    Input:
        - orig: the coordinate of enu frame's origin in ECEF
        - pos : (x,y,z), the location of the point represent in ENU
    Output:
        pos: (X,Y,Z), a point represent in ECEF
    """
    X_o = orig[0]
    Y_o = orig[1]
    Z_o = orig[2]
    x = pos[0]
    y = pos[1]
    z = pos[2]

    orig_gps = cc_ecef2geodetic(orig)

    lat = math.radians(orig_gps[0])
    lon = math.radians(orig_gps[1])
    alt = orig_gps[2]

    X = -math.sin(lon) * x - math.sin(lat) * math.cos(lon) * \
        y + math.cos(lat) * math.cos(lon) * z + X_o
    Y = math.cos(lon) * x - math.sin(lat) * math.sin(lon) * \
        y + math.cos(lat) * math.sin(lon) * z + Y_o
    Z = math.cos(lat) * y + math.sin(lat) * z + Z_o

    return X, Y, Z


def cc_geodetic2enu(orig_gps, gps):
    """
    Convert a geodetic coordinate to coordinate in local ENU frame.

    Input:
        - orig_gps: the gps of enu frame's origin
        - gps: (lat, lon, alt)
    Output:
        pos: (x,y,z), the location of the point represent in ENU
    """
    orig_ecef = cc_geodetic2ecef(orig_gps)
    pos_ecef = cc_geodetic2ecef(gps)

    pos = cc_ecef2enu(orig_ecef, pos_ecef)
    return pos


def cc_enu2geodetic(orig_gps, pos):
    """
    Convert a coordinate in local ENU frame to geodetic coordinate.

    Input:
        - orig_gps: the gps of enu frame's origin
        - pos : (x,y,z), the location of the point represent in ENU
    Output:
        gps: (lat, lon, alt)
    """
    orig_ecef = cc_geodetic2ecef(orig_gps)
    pos_ecef = cc_enu2ecef(orig_ecef, pos)

    gps = cc_ecef2geodetic(pos_ecef)
    return gps


def test1():
    '''
    WGS84 and ECEF
    '''
    gps = (30.123456, 109.123456, 100)
    # gps = (0.0, 90.0, 100)                  # 赤道表面，经度=90
    # gps = (0.0, 0.0, 100)                   # 赤道表面，经度=0
    # gps = (90.0, 0.0, 100)                # 北极点
    pos = cc_geodetic2ecef(gps)
    print(pos)
    #
    gps = cc_ecef2geodetic(pos)
    print(gps)


def test2():
    '''
    ENU and ECEF 
    '''
    gps = (0.0, 0.0, 100)
    orig = (6378237.0, 0.0, 0.0)

    pos_enu = (20.0, 50.0, 10.0)
    #
    pos_ecef = cc_enu2ecef(orig, pos_enu)
    print(f"pos_ecef={pos_ecef}")
    #
    pos_enu = cc_ecef2enu(orig, pos_ecef)
    print(f"pos_enu={pos_enu}")


def test3():
    '''
    ENU and WGS84
    '''
    orig_gps = (30.123456, 109.123456, 0.0)
    gps = (30.123256, 109.123456, 10.0)

    pos_enu = cc_geodetic2enu(orig_gps, gps)
    print(f"pos_enu={pos_enu}")
    #
    pos_enu = (0.0, 1000.0, 0.0)
    gps = cc_enu2geodetic(orig_gps, pos_enu)
    print(f"gps={gps}")


def main(args=None):
    # test1()
    # test2()
    test3()


if __name__ == '__main__':
    main()
